Circle_Path = Path(200);
fprintf('trajectory size %d \n',Circle_Path.Traject_Size);
S_P = Point();
S_P.x = 5;
S_P.y = 20;
S_P.yaw = pi/3;
turn_a = pi/6;
mode = 1;
t_rate = pi/12;
Circle_Path.Generating_Cycle_Path(S_P,turn_a,mode,t_rate);
fprintf('trajectory size %d \n',Circle_Path.Traject_Size);
axis([0 40 0 40]);
hold on;
for i = 1:Circle_Path.Traject_Size
    scatter(Circle_Path.Trajectory(1,i).x,Circle_Path.Trajectory(1,i).y,'red','.');
    hold on;
    pause(0.001);
%     fprintf('trajectory %d x %f y %f \n',i,Circle_Path.Trajectory(1,i).x,Circle_Path.Trajectory(1,i).y);
end

test_arr = ARR_Kinematic_Model(2,23,-2*pi/3,0,1);
Vehicle_fPoint =  test_arr.P_f;
Vehicle_rPoint =  test_arr.P_r;
Closest_Index = Circle_Path.Find_Closest_Point(1,Vehicle_fPoint,Vehicle_rPoint,mode);
fprintf('Closest_Index  %d x %f y %f\n',Closest_Index,Circle_Path.Trajectory(1,Closest_Index).x,Circle_Path.Trajectory(1,Closest_Index).y);
scatter(Vehicle_fPoint.x,Vehicle_fPoint.y,'blue');
hold on;
scatter(Vehicle_rPoint.x,Vehicle_rPoint.y,'green');
hold on;
scatter(test_arr.P_m.x,test_arr.P_m.y,'black','.');
hold on;
scatter(Circle_Path.Trajectory(1,Closest_Index).x,Circle_Path.Trajectory(1,Closest_Index).y,'red');
hold on;